/*********************************************************
*
*  FJH-sim v1.0
*  Copyright � 2008 H.Y. Bruintjes, J.H. Donker,
*  C.E. Etheredge, J. Teeuw
*
*  Simulator core
*
**********************************************************/

#include "simulator/Simulator.h"

#include <cmath>
#include <ctime>
#include <cstdlib>
#include <iostream>

#include "node/SimNode.h"
#include "node/EyeNode.h"
#include "protocol/SimulatorProtocolStack.h"

#include "shared/Utils.h"

namespace simulator
{

unsigned int dice(unsigned numberOfEyes)
{
	return (unsigned int)(rand() % numberOfEyes);
}

float Driver::distanceBetweenPoints(const Position a, const Position b)
{
	float dx = (a.x - b.x);
	float dy = (a.y - b.y);
	float dz = (a.z - b.z);
	
	return sqrt(dx*dx + dy*dy + dz*dz);
}

Simulator::Simulator(float speed_, float range_, int loseRate_, int mangleRate_, Driver* driver_)
{
	speed = speed_;
	range = range_;
	loseRate = loseRate_;
	mangleRate = mangleRate_;
	driver = driver_;
	driver->setSimulator(this);
	fout = new std::ofstream();
	//stats
	mangled = 0;
	lost = 0;
	added = 0;
	delivered = 0;
	collision = 0;
	arrived = 0;
	ticks = 0;
	fout->open("simulator.log");
	
	EventNode::resetID();
}

void Simulator::writeToLog(std::string msg, Event* e)
{
	// ACHTUNG: CRASH!
	return;
	
	char* data = (char*) e->getPacket().data();
	
	using namespace std;
	(*fout) << msg << " at tick count " << ticks << endl;

	PacketHeader* p = (PacketHeader*) data;
	
	(*fout) << "    Header (" << sizeof(PacketHeader) << " bytes): " << endl;
	(*fout) << "        magick = " << hex << p->magick << endl;
	(*fout) << "        destination = " << hex << p->destination << endl;
	(*fout) << "        nexthop = " << hex << p->nexthop << endl;
	(*fout) << "        source = " << hex << p->source << endl;
	(*fout) << "        plength = " << dec << p->plength << endl;
	(*fout) << "        ttl = " << dec << p->ttl << endl;
	(*fout) << "        tos = " << dec << p->tos << endl;
	(*fout) << "        options = " << hex << p->options << endl;
	(*fout) << "        hckecksum = " << hex << p->hchecksum << endl;
	
	// packet data
	(*fout) << dec << "    Payload (" << p->plength << " bytes): " << hex << endl;

    int offset = sizeof(PacketHeader);
	for(unsigned int i = 0; i < p->plength; i ++) {
        if (i%8 == 0)
            (*fout) << "        ";
		unsigned char d = (unsigned char) data[offset + i];
        if (d < 0x10)
            (*fout) << 0;
		(*fout) << (int)d << " ";
        if (i%8 == 7)
           (*fout) << endl;
	}
    (*fout) << endl;

	(*fout) << dec << endl;
	
	/*
	PacketHeader* p = (PacketHeader*) e->getPacket().data();
	(*fout) << p->magick <<":" << p->destination << ":" << p->nexthop << ":"<< p->source << ":" ;
	(*fout) << p->plength << ":" << p->ttl << ":" << p->tos << ":" << p->options <<":" << p->hchecksum <<" || ";

	for(unsigned int i=0; i < p->plength ;i++)
	{
		(*fout)<< (unsigned int) e->getPacket()[i+sizeof(PacketHeader)] << ":";
	}

	(*fout) << dec << endl;
	*/
}

void Simulator::run(unsigned int turns)
{
	using namespace std;
	// main application loop
	
	for(unsigned int i=0; i < turns; i++) 
	{
		driver->waitForContinue();
		//cout << endl;
		//cout << "Turn:" << ticks << endl;	
		debug_print("Simulator Core", "");
		debug_print("Simulator Core", "##################################");
		debug_print("Simulator Core", "New step (%u)", ticks);
		debug_print("Simulator Core", "##################################");
				
		//handle events
		debug_print("Simulator Core", "Processing events on event queue");
		handleEvents();
		debug_print("Simulator Core", "Done processing events on event queue");

		debug_print("Simulator Core", "Inject events in nodes");
		for(vector<EventNode*>::iterator itr = nodes.begin(); itr != nodes.end(); itr++)
		{
			(*itr)->tick();
		}

		driver->sendDrive();
		
		debug_print("Simulator Core", "Allow each node to perform one tick of work");
		for(vector<SimulatorProtocolStack*>::iterator itr = stacks.begin(); itr != stacks.end(); itr++)
		{
			debug_print("Simulator Core", "");
			debug_print("Simulator Core", "  ---------------------------------");
			debug_print("Simulator Core", " Node %u at the Simulator Core", (*itr)->getNodeAddress());
			debug_print("Simulator Core", "  ---------------------------------");
			
			// update protocol stack
			(*itr)->update();
		}


		driver->receiveDrive();
		//printStat();

		// update tick count
		ticks++;
	}
}

void Simulator::handleEvents()
{
	//handle all pending events. First remove some, then (apply corruption and) deliver them
	using namespace std;
	
	//Drop packets because of errors or because they expire
	vector<Event *>::iterator eventsBegin = events.begin();
//	cout << "\tTraversal state (packetloss)" << endl;
	for(int i = events.size() - 1; i >= 0; i--)
	{
		Event * pEvent = events.at(i);
		float travel = pEvent->getRadius();
		
		if (travel >= range)	//event has died
		{
			debug_print("Simulator Core", "Removing event: %d", i);
			
			driver->notifyOutOfRange(events[i]);
			
			events.erase(eventsBegin+i);
			continue;
		}
		
		// Update event radius
		travel = min(range+0.1f, (travel+speed));	//add small 0.1f to allow float imprecisions when comparing
		pEvent->setRadius (travel);
		
		if ( (loseRate) > dice(100) )	//
		{
			debug_print("Simulator Core", "Losing event: %d", i);
			
			driver->notifyLose(events[i]);
			
			events.erase(eventsBegin+i);
			lost++;
			continue;
		}
		//otherwise, just continue looping
	}
	
	
	//deliver packets, possibly mangle packets
//	cout << "\tDeliver stage (corruption)" << endl;
	for(int i = events.size() - 1; i >= 0; i--)
	{
		Event * pEvent = events.at(i);
		float travel = float(ticks - pEvent->getBeginTime()) * speed;
		float travelPrev = float(ticks - pEvent->getBeginTime() - 1) * speed;

		if ( (mangleRate) > dice(100) )		//
		{
			debug_print("Simulator Core", "Mangling event: %d", i);
			pEvent->mangle();
			
			driver->notifyMangle(pEvent);

			mangled++;
		}
		
		for(unsigned int j=0; j < nodes.size(); j++)
		{
			float distance = driver->distanceBetweenPoints(pEvent->getPosition(), nodes[j]->getPosition());
			if ((travelPrev <= distance) && (distance < travel) && (distance > 0))
			{
				debug_print("Simulator Core", "Delivering event: %d", i);
				writeToLog("Deliver Event", pEvent);
				
				//(Temporarily?) prevent event from arriving at node it originated from.
				if (pEvent->_sourceID == (unsigned int)nodes[j])
					continue;
				
				if (nodes[j]->eventArrive(pEvent))
				{
//					cout << "\t\t\tDelivery caused collision" << endl;
					driver->notifyCollision(nodes[j]);
					collision++;
					
				}
				else
				{	
					

					driver->notifyArrive(nodes[j]);
					delivered++;
				}
				driver->recieveEventAtNode(pEvent);				
				arrived++;
			}			

		}

	}
}

float Simulator::getSpeed()
{
	return speed;
}

float Simulator::getRange()
{
	return range;
}

void Simulator::setSpeed(float speed_)
{
	speed = speed_;
}

void Simulator::setRange(float range_)
{
	range = range_;
}

void Simulator::setLoseRate(unsigned int loseRate_)
{
	loseRate = loseRate_;
}


void Simulator::setMangleRate(unsigned int mangleRate_)
{
	mangleRate = mangleRate_;
}


unsigned int Simulator::getMangleRate()
{
	return mangleRate;
}

unsigned int Simulator::getLoseRate()
{
	return loseRate;
}

void Simulator::getFriends ( EventNode* e, std::vector<EventNode *>& list )
{
	// Get the list of node addresses from the protocol stack
	std::vector < NodeAddress > addresses;
	getProtocolStack (e)->getFriends (addresses);
	
	// Loop through all nodes and add all matching node addresses
	for(std::vector<EventNode*>::iterator itrn = nodes.begin(); itrn != nodes.end(); itrn++)
		for(std::vector<NodeAddress>::iterator itra = addresses.begin(); itra != addresses.end(); itra++)
			if ( (*itrn)->getNode ()->getNodeAddress () == *itra )
				list.push_back ( *itrn );
}

void Simulator::getAcquaintances ( EventNode* e, std::vector<EventNode *>& list )
{
	// Get the list of node addresses from the protocol stack
	std::vector < NodeAddress > addresses;
	getProtocolStack (e)->getAcquaintances (addresses);
	
	// Loop through all nodes and add all matching node addresses
	for(std::vector<EventNode*>::iterator itrn = nodes.begin(); itrn != nodes.end(); itrn++)
		for(std::vector<NodeAddress>::iterator itra = addresses.begin(); itra != addresses.end(); itra++)
			if ( (*itrn)->getNode ()->getNodeAddress () == *itra )
				list.push_back ( *itrn );
}


SimulatorProtocolStack* Simulator::getProtocolStack(unsigned int i)
{
	if(i > stacks.size()) {
		printf("HELP!\n");
		return NULL;
	}
	return stacks[i];
}

SimulatorProtocolStack* Simulator::getProtocolStack(EventNode *n)
{
	for(unsigned int i = 0; i < nodes.size(); i++)
	{
		if(nodes[i] == n)
			return stacks[i];
	}
	return NULL;
}

EventNode* Simulator::getNode(unsigned int i)
{
	if(i > nodes.size()) {
		printf("HELP!\n");
		return NULL;
	}
	return nodes[i];
}

EventNode* Simulator::getNodeFromAddr(NodeAddress addr)
{
	for(size_t i = 0; i < nodes.size(); i++) {
		if (nodes[i]->getNode()->getNodeAddress() == addr) {
			return nodes[i];
		}
	}
	return 0;
}

void Simulator::addNode(EventNode* n)
{
	nodes.push_back(n);
	stacks.push_back(new SimulatorProtocolStack(n->getNode()));
}

void Simulator::addEvent(Event* e)
{
	events.push_back(e);
	driver->notifySend(e);
	driver->addEventToSimulator(e);
	writeToLog("Add Event", e);
	added++;
}

void Simulator::removeNode(EventNode* n)
{
	for(unsigned int i = 0; i < nodes.size(); i++)
	{
		if(nodes[i] == n) {
			nodes.erase(nodes.begin() + i);
			stacks.erase(stacks.begin() + i);
			return;
		}
	}
}

unsigned int Simulator::getNumberOfNodes()
{
	return nodes.size();
}

unsigned int Simulator::getNumberOfEvents()
{
	return events.size();
}

unsigned int Simulator::getNumberOfStacks()
{
	return stacks.size();
}

void Simulator::printStat()
{
	using namespace std;
	cout << endl;
	cout << endl;
	cout << "Simulator Core - Status Overview" << endl;
	cout << "  Turns run: " <<ticks << endl;
	cout << endl;	
	cout << "  Events status" << endl;
	cout << "    Added:" << added << endl;
	cout << "    Delivered:" << delivered << endl;
	cout << "    Lost:" <<	lost << endl;
	cout << "    Mangled:" << mangled << endl;
	cout << "    Collisions:" << collision << endl;
	cout << "    Arrived:" << arrived << endl;
	cout << endl;
}

unsigned long Simulator::getTicks()
{
	return ticks;	
}

Simulator::~Simulator()
{
	fout->close();
	delete fout;
	//TODOwalk trough vectors and kill them
}

unsigned long EventNode::NextSimID = 0;

EventNode::EventNode(Simulator* sim_, bool eyeNode)
{
	sim = sim_;
	_userdata = NULL;
	if (eyeNode)
		_pNode = new EyeNode((NodeAddress)EventNode::NextSimID++ + 0x80);
	else
		_pNode = new SimNode((NodeAddress)EventNode::NextSimID++);
	_pNode->open();
	_id = EventNode::NextSimID;
	std::cout << "created :" << _id << std::endl;
	_pNode->registerListener(this);
}

EventNode::EventNode(float _x, float _y,float _z, Simulator* sim_, bool eyeNode)
{
	sim = sim_;
	position = Position(_x, _y, _z);
	if (eyeNode)
		_pNode = new EyeNode((NodeAddress)EventNode::NextSimID++ + 0x80);
	else
		_pNode = new SimNode((NodeAddress)EventNode::NextSimID++);
	_id = EventNode::NextSimID;
	std::cout << "created :" << _id << std::endl;		
	_pNode->open();
	_pNode->registerListener(this);
}

Position EventNode::getPosition()
{
	return position;
}

void EventNode::setPosition(Position position_)
{
	position = position_;
}

bool EventNode::eventArrive(Event* event)
{
	using namespace std;
	receivedEvents.push_back(event);

	bool collision = false;
	unsigned int lastID = receivedEvents.at(0)->_sourceID;
	for(size_t i = 0; i < receivedEvents.size(); i++) {
		if (lastID != receivedEvents.at(i)->_sourceID) {
			collision = true;
			break;
		}
	}
	return collision;//(receivedEvents.size() > 1);
}

std::vector<std::string> EventNode::interleave()
{
	using namespace std;
	vector<string> buffer;
	  
	//find the maximum length
	unsigned int length = 0;
	
	if (receivedEvents.size() > 1)
	{
		//check if the events are all from the same source. If so, no collision could possibly have occured
		bool canCollide = false;
		unsigned int lastID = receivedEvents.at(0)->_sourceID;
		for(size_t i = 0; i < receivedEvents.size(); i++) {
			if (lastID != receivedEvents.at(i)->_sourceID) {
				canCollide = true;
				break;
			}
		}
		
		if (canCollide == false) {
			for(size_t i = 0; i < receivedEvents.size(); i++) {
				buffer.push_back(receivedEvents[i]->getPacket());
			}
		} else {
		
			if (dice(100) > 90) {	//10% chance of interleave
				//find the maximum
				for(vector<Event*>::iterator itr = receivedEvents.begin(); itr != receivedEvents.end(); itr++)
				{
					if (length < (*itr)->getPacket().size())
					{
						length = (*itr)->getPacket().size();
					}
				}
				
				
				// randomly take a sample from the strings and add this to the buffer
				// until the entire buffer is filled.
				string garbage = "";
				for(unsigned int i = 0; i < length; i++)
				{
					bool found = false;
					while(!found)
					{
						//cout << "interleave loop" << endl;	
						unsigned int j = dice(receivedEvents.size());
						//cout << recievedEvents.size() <<"->" << j << endl;
						if (receivedEvents[j]->getPacket().size() > i)
						{
							char c = receivedEvents[j]->getPacket()[i];
							garbage+=c;
							found = true;
						}
						
					}
				}
				buffer.push_back(garbage);
			} else {	//90% chance of garbage detection
				buffer.push_back("");
			}
		
		}
	}
	// Just one message came trough pass it unharmed
	else if (receivedEvents.size() == 1)
	{
		//cout << "RIGHT PATH" << endl;
		buffer.push_back(receivedEvents[0]->getPacket());
	}
	// Nothing appeared just return an empty string
	else
		buffer.push_back("");
	return buffer;
}

void EventNode::tick()
{
	using namespace std;
	vector<string> buffer = interleave();
	for(size_t i = 0; i < buffer.size(); i++) {
		_pNode->inject(buffer[i].data(), buffer[i].length());	
	}
	receivedEvents.clear();
}

Node* EventNode::getNode()
{
	return _pNode;
}

Simulator* EventNode::getSimulator()
{
	return sim;
}

void EventNode::dataArrived(const char * data, int len) {
	std::string buffer(data, len);
	Event* e= new Event(*this, buffer, getSimulator()->getTicks());
	getSimulator()->addEvent(e);
}

EventNode::~EventNode()
{
	_pNode->registerListener(NULL);
	using namespace std;
	if (_pNode) {
		delete _pNode;
	}
}

Event::Event(EventNode& node_, std::string packet_, unsigned long beginTime_)
{
	position = node_.getPosition();
	packet = packet_;
	beginTime = beginTime_;
	radius = 0;
	_userdata = NULL;
	_sourceID = (unsigned int)&node_;	//just cast the pointer to something useful
}

void Event::mangle()
{
	using namespace std;
	//roll a dice for positon
	unsigned int i = dice(packet.size() -1);
	//roll a dice to screw the bitss
	packet[i] = (char)dice(256);
}


unsigned long Event::getBeginTime()
{
	return beginTime;
}

Position Event::getPosition()
{
	return position;
}


std::string Event::getPacket()
{
	return packet;
}

Event::~Event()
{
}


void Driver::run(int runs)
{
	using namespace std;
	
	debug_print("Simulator Core", "Ready to run!");
	if (runs == -1)
		for(;;)
			sim->run(1);
	else
		sim->run(runs);
}

void Driver::setSimulator(Simulator* sim_)
{
	sim = sim_;
}


void Driver::addEventToSimulator(Event* e)
{
}

void Driver::recieveEventAtNode(Event* e)
{
}

void Driver::notifyCollision( EventNode* node )
{
}

void Driver::notifyMangle( Event* event )
{
}

void Driver::notifyArrive( EventNode* node )
{
}

void Driver::notifySend( Event* event )
{
}

void Driver::notifyLose( Event* event )
{
}

void Driver::notifyOutOfRange( Event* event )
{
}

Driver::~Driver()
{
}	
} //end of namespace Simulator, not stray curly bracket :P

